#include "Viewer.h"
#include <unistd.h>
#include <pangolin/pangolin.h>




using namespace std;

Viewer::Viewer(MapDrawer *pMapDrawer, float mViewpointX, float mViewpointY, float mViewpointZ, float mViewpointF, float fps):
	m_MapDrawer(pMapDrawer),
	m_ViewpointX(mViewpointX),
	m_ViewpointY(mViewpointY),
	m_ViewpointZ(mViewpointZ),
	m_ViewpointF(mViewpointF),
	m_FinishRequested(false),
	m_Finished(true),
	m_Stopped(true),
	m_StopRequested(false)
{
	if(fps<1)
	{
		fps=30;
	}
	m_T = 1e3 / fps;
}


Viewer::~Viewer()
{

}


void Viewer::RequestFinish()
{
	unique_lock<mutex> lock(m_MutexFinish);
    m_FinishRequested = true;
}

void Viewer::RequestStop()
{
	unique_lock<mutex> lock(m_MutexStop);
	if(!m_Stopped)
	{
		m_StopRequested = true;
	}
}

bool Viewer::isFinished()
{
	unique_lock<mutex> lock(m_MutexFinish);
	return m_Finished;
}


bool Viewer::isStopped()
{
	unique_lock<mutex> lock(m_MutexStop);
	return m_Stopped;
}


void Viewer::Release()
{
	unique_lock<mutex> lock(m_MutexStop);
    m_Stopped = false;
}


bool Viewer::Stop()
{
	unique_lock<mutex> lock(m_MutexStop);
	unique_lock<mutex> lock2(m_MutexFinish);
	if(m_FinishRequested)
	{
		return false;
	}
	else if(m_StopRequested)
	{
		m_Stopped = true;
		m_StopRequested = false;
		return true;
	}

	return false;
}


bool Viewer::CheckFinish()
{
	unique_lock<mutex> lock(m_MutexFinish);
	return m_FinishRequested;
}


void Viewer::SetFinish()
{
	unique_lock<mutex> lock(m_MutexFinish);
    m_Finished = true;
}


void Viewer::Run()
{
	m_Finished = false;
	m_Stopped = false;

	pangolin::CreateWindowAndBind("SIFT-SLAM: Map Viewer",1024,768);

	// 3D Mouse handler requires depth testing to be enabled
	glEnable(GL_DEPTH_TEST);

	// Issue specific OpenGl we might need
	glEnable (GL_BLEND);
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
	pangolin::Var<bool> menuFollowCamera("menu.Follow Camera",true,true);
	pangolin::Var<bool> menuShowPoints("menu.Show Points",true,true);
	pangolin::Var<bool> menuShowKeyFrames("menu.Show KeyFrames",true,true);
	pangolin::Var<bool> menuShowGraph("menu.Show Graph",true,true);
	pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
	pangolin::Var<bool> menuReset("menu.Reset",false,false);

	// Define Camera Render Object (for view / scene browsing)
	pangolin::OpenGlRenderState s_cam(
				pangolin::ProjectionMatrix(1024, 768, m_ViewpointF, m_ViewpointF, 512, 389, 0.1, 1000),
				pangolin::ModelViewLookAt(m_ViewpointX, m_ViewpointY, m_ViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0)
				);

	// Add named OpenGL viewport to window and provide 3D Handler
	pangolin::View& d_cam = pangolin::CreateDisplay()
			.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)
			.SetHandler(new pangolin::Handler3D(s_cam));

	pangolin::OpenGlMatrix Twc;
	Twc.SetIdentity();

	bool bFollow = true;
	bool bLocalizationMode = false;

	while(1)
	{
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		m_MapDrawer->GetCurrentOpenGLCameraMatrix(Twc);

		if(menuFollowCamera && bFollow)
		{
			s_cam.Follow(Twc);
		}
		else if(menuFollowCamera && !bFollow)
		{
			s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(m_ViewpointX, m_ViewpointY, m_ViewpointZ, 0,0,0,0.0,-1.0, 0.0));
			s_cam.Follow(Twc);
			bFollow = true;
		}
		else if(!menuFollowCamera && bFollow)
		{
			bFollow = false;
		}

		if(menuLocalizationMode && !bLocalizationMode)
		{
			bLocalizationMode = true;
		}
		else if(!menuLocalizationMode && bLocalizationMode)
		{
			bLocalizationMode = false;
		}

		d_cam.Activate(s_cam);
		glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
		m_MapDrawer->DrawCurrentCamera(Twc);
		if(menuShowKeyFrames || menuShowGraph)
		{
			m_MapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph);
		}

		if(menuShowPoints)
		{
			m_MapDrawer->DrawMapPoints();
		}

		pangolin::FinishFrame();

		if(menuReset)
		{
			menuShowGraph = true;
			menuShowKeyFrames = true;
			menuShowPoints = true;
			menuLocalizationMode = false;

			bLocalizationMode = false;
            bFollow = true;
            menuFollowCamera = true;
            menuReset = false;
		}

		if(Stop())
		{
			while(isStopped())
			{
				usleep(3000);
			}
		}

		if(CheckFinish())
		{
			break;
		}		
	}

	SetFinish();
}

